#include <rclcpp/rclcpp.hpp>
#include <unordered_map>
#include <sensor_msgs/msg/joy.hpp>

#include <string>

class JoystickReader : public rclcpp::Node {
public:
    JoystickReader() : Node("joystick_reader_node") {
        // 在类的构造函数中声明所有参数
        
        this->declare_parameter("axis_mapping.left_stick_x", 0);    // 左摇杆水平
        this->declare_parameter("axis_mapping.left_stick_y", 1);    // 左摇杆垂直  
        this->declare_parameter("axis_mapping.LT", 2);              // 左扳机键
        this->declare_parameter("axis_mapping.RT", 5);              // 右扳机键
        this->declare_parameter("axis_mapping.right_stick_x", 3);   // 右摇杆水平
        this->declare_parameter("axis_mapping.right_stick_y", 4);   // 右摇杆垂直
        this->declare_parameter("axis_mapping.dpad_up", 7);         // 方向键上
        this->declare_parameter("axis_mapping.dpad_left", 6);       // 方向键左

        this->declare_parameter("button_mapping.A", 0);             // A按钮
        this->declare_parameter("button_mapping.B", 1);             // B按钮
        this->declare_parameter("button_mapping.X", 2);             // X按钮
        this->declare_parameter("button_mapping.Y", 3);             // Y按钮
        this->declare_parameter("button_mapping.LB", 4);            // 左肩键
        this->declare_parameter("button_mapping.RB", 5);            // 右肩键
        this->declare_parameter("button_mapping.start", 7);         // 开始键
        this->declare_parameter("button_mapping.select", 6);        // 选择键
        this->declare_parameter("button_mapping.M1", 9);            // M1键
        this->declare_parameter("button_mapping.M2", 10);           // M2键
        this->declare_parameter("button_mapping.home", 8);          // Home键
        // 先列出所有参数名，限定前缀为 axis_mapping
        auto param_names = this->list_parameters({"axis_mapping"}, 10).names;
       
        for (const auto & full_name : param_names) {
            // full_name 形如 axis_mapping.left_stick_x
            if (this->has_parameter(full_name)) {
                auto param = this->get_parameter(full_name);
                // 去掉前缀 "axis_mapping."
                std::string key = full_name.substr(strlen("axis_mapping.") );
                axis_map_[key] = param.as_int();
            }
        }

        auto button_param_names = this->list_parameters({"button_mapping"}, 10).names;
        for (const auto & full_name : button_param_names) {
            if (this->has_parameter(full_name)) {
                auto param = this->get_parameter(full_name);
                std::string key = full_name.substr(strlen("button_mapping."));
                button_map_[key] = param.as_int();
            }
        }

       
        printparam();
        joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
            "joy", 10,
            std::bind(&JoystickReader::joyCallback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "C++ Joystick reader node initialized");
        last_buttons_.resize(11, 0);

        

      
    }

private:
    void printparam(){
    // 打印测试
        RCLCPP_INFO(this->get_logger(), "Axis Mapping:");
        for (const auto &pair : axis_map_) {
            RCLCPP_INFO(this->get_logger(), "  %s: %d", pair.first.c_str(), pair.second);
        }

        RCLCPP_INFO(this->get_logger(), "Button Mapping:");
        for (const auto &pair : button_map_) {
            RCLCPP_INFO(this->get_logger(), "  %s: %d", pair.first.c_str(), pair.second);
        }
    }
    void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy)
    {
        for (const auto &pair : axis_map_)
        {
            const std::string &name = pair.first;
            int index = pair.second;

            if (index >= 0 && index < joy->axes.size())
            {
                float value = joy->axes[index];
                if (index==2 || index ==5){
                    if (std::abs(value) < 0.95)
                    RCLCPP_INFO(this->get_logger(), "Mapped Axis %s: %.2f", name.c_str(), value);
                }else{
                     if (std::abs(value) > 0.05)
                    RCLCPP_INFO(this->get_logger(), "Mapped Axis %s: %.2f", name.c_str(), value);
                }
               
            }
        }
        
        // 按钮处理（边缘触发）
        for (const auto &pair : button_map_)
        {
            const std::string &name = pair.first;
            int index = pair.second;

            if (index >= 0 && index < joy->buttons.size())
            {
                int curr = joy->buttons[index];
                int last = (index < last_buttons_.size()) ? last_buttons_[index] : 0;

                if (curr == 1 && last == 0)
                {
                    RCLCPP_INFO(this->get_logger(), "Mapped Button %s PRESSED ", name.c_str());
                }
                // else if (curr == 0 && last == 1)
                // {
                //     RCLCPP_INFO(this->get_logger(), "Mapped Button %s RELEASED (↓下降沿)", name.c_str());
                // }

                // 更新 last_buttons_
                if (index >= last_buttons_.size())
                    last_buttons_.resize(index + 1, 0);
                last_buttons_[index] = curr;
            }
        }


        // for (const auto &pair : button_map_)
        // {
        //     const std::string &name = pair.first;
        //     int index = pair.second;

        //     if (index >= 0 && index < joy->buttons.size() && joy->buttons[index])
        //     {
        //         RCLCPP_INFO(this->get_logger(), "Mapped Button %s PRESSED", name.c_str());
        //     }
        // }
    }
    std::unordered_map<std::string, int> axis_map_;
    
    std::unordered_map<std::string, int> button_map_;
    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
    std::vector<int> last_buttons_;


};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<JoystickReader>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
